#include <iostream>
#include <vector>
#include <cmath>
#include <yaml-cpp/yaml.h>
#include "rtk_data_path.h"
#include "common_tool/csv_parser.h"
#include "common_tool/csv_writer.h"
#include "matplotlibcpp.hpp"
namespace plt = matplotlibcpp;
bool circleLeastFit(const std::vector<double> &x_vec, const std::vector<double> &y_vec, double &center_x, double &center_y, double &radius)
{
	center_x = 0.0f;
	center_y = 0.0f;
	radius = 0.0f;
	if (x_vec.size() < 3 || (x_vec.size() != y_vec.size()))
	{
		std::cout << "circleLeastFit failed,x_vec.size() < 3 || (x_vec.size() != y_vec.size()) " << std::endl;
		return false;
	}

	double sum_x = 0.0f, sum_y = 0.0f;
	double sum_x2 = 0.0f, sum_y2 = 0.0f;
	double sum_x3 = 0.0f, sum_y3 = 0.0f;
	double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

	int N = x_vec.size();
	for (int i = 0; i < N; i++)
	{
		double x = x_vec[i];
		double y = y_vec[i];
		double x2 = x * x;
		double y2 = y * y;
		sum_x += x;
		sum_y += y;
		sum_x2 += x2;
		sum_y2 += y2;
		sum_x3 += x2 * x;
		sum_y3 += y2 * y;
		sum_xy += x * y;
		sum_x1y2 += x * y2;
		sum_x2y1 += x2 * y;
	}

	double C, D, E, G, H;
	double a, b, c;

	C = N * sum_x2 - sum_x * sum_x;
	D = N * sum_xy - sum_x * sum_y;
	E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
	G = N * sum_y2 - sum_y * sum_y;
	H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

	center_x = a / (-2);
	center_y = b / (-2);
	radius = std::sqrt(a * a + b * b - 4 * c) / 2;
	return true;
}
int main()
{
	std::string config_file = rtk_config_path + "u_turn_record.yaml";
	YAML::Node config_yaml = YAML::LoadFile(config_file); //也可以这样读取文件
	std::string file_name = rtk_data_path + "u_turn_record_daxia.csv";
	io::CSVReader<3> record_csv(file_name);
	record_csv.read_header(io::ignore_extra_column, "x", "y", "theta");
	std::vector<double> x_vec;
	std::vector<double> y_vec;
	std::vector<double> theta_vec;

	std::vector<double> x_org_vec;
	std::vector<double> y_org_vec;
	std::vector<double> theta_org_vec;

	double x;
	double y;
	double theta;
	while (record_csv.read_row(x, y, theta))
	{
		x_org_vec.push_back(x);
		y_org_vec.push_back(y);
		theta_org_vec.push_back(theta);
		if ((theta <= 180.0 && theta >= 50.0) || (theta <= -55 && theta >= -180.0))
		{
			x_vec.push_back(x);
			y_vec.push_back(y);
			theta_vec.push_back(theta);
		}
	}
	double center_x = 0;
	double center_y = 0;
	double radius = 0;
	if (circleLeastFit(x_vec, y_vec, center_x, center_y, radius) == false)
	{
		std::cout << "circleLeastFit failed" << std::endl;
		return false;
	}
	std::cout << "ceter x is:" << center_x << " center_y is:" << center_y << " radius is:" << radius << std::endl;
	//circle shape
	double delta_s = 0.2;
	int num = 2 * M_PI * radius / delta_s;
	double delta_theta = 2 * M_PI / num;
	std::vector<double> x_cir_vec;
	std::vector<double> y_cir_vec;
	std::vector<double> theta_cir_vec;
	std::vector<double> kappa_cir_vec;
	std::vector<double> s_cir_vec;
	double start_angle = config_yaml["start_angle"].as<double>() / 180.0 * M_PI;
	double end_angle = config_yaml["end_angle"].as<double>() / 180.0 * M_PI;

	//前面插入直线
	double start_ex_x = center_x + radius * std::cos(start_angle);
	double start_ex_y = center_y + radius * std::sin(start_angle);

	/*x_cir_vec.insert(x_cir_vec.begin(), start_ex_x);
	y_cir_vec.insert(y_cir_vec.begin(), start_ex_y);
	theta_cir_vec.insert(theta_cir_vec.begin(), 90 + start_angle * 180 / M_PI);
	kappa_cir_vec.insert(kappa_cir_vec.begin(), 1.0 / radius);*/

	double ex_start_s = config_yaml["start_length"].as<double>();
	int num_start = ex_start_s / delta_s;

	for (int i = 0; i < num_start - 1; i++)
	{
		double temp_x = start_ex_x + i * delta_s * std::cos(start_angle - 0.5 * M_PI);
		double temp_y = start_ex_y + i * delta_s * std::sin(start_angle - 0.5 * M_PI);
		x_cir_vec.insert(x_cir_vec.begin(), temp_x);
		y_cir_vec.insert(y_cir_vec.begin(), temp_y);
		theta_cir_vec.insert(theta_cir_vec.begin(), 90 + start_angle * 180 / M_PI);
		kappa_cir_vec.insert(kappa_cir_vec.begin(), 0.0);
	}

	for (int i = 1; i < num - 1; i++)
	{

		double temp_theta = start_angle + i * delta_theta;
		std::cout << start_angle << std::endl;
		if (temp_theta < end_angle)
		{
			x_cir_vec.push_back(center_x + radius * std::cos(temp_theta));
			y_cir_vec.push_back(center_y + radius * std::sin(temp_theta));
			theta_cir_vec.push_back(90 + temp_theta * 180 / M_PI);
			kappa_cir_vec.push_back(1.0 / radius);
		}
	}

	//后面插入直线
	double end_ex_x = center_x + radius * std::cos(end_angle);
	double end_ex_y = center_y + radius * std::sin(end_angle);

	/*x_cir_vec.push_back(end_ex_x);
	y_cir_vec.push_back(end_ex_y);
	theta_cir_vec.push_back(90 + end_angle * 180 / M_PI);
	kappa_cir_vec.push_back(1.0 / radius);*/

	double ex_end_s = config_yaml["end_length"].as<double>();
	int num_end = ex_end_s / delta_s;
	for (int i = 0; i < num_end - 1; i++)
	{
		double temp_x = end_ex_x + i * delta_s * std::cos(end_angle + 0.5 * M_PI);
		double temp_y = end_ex_y + i * delta_s * std::sin(end_angle + 0.5 * M_PI);
		x_cir_vec.push_back(temp_x);
		y_cir_vec.push_back(temp_y);
		theta_cir_vec.push_back(90 + end_angle * 180 / M_PI);
		kappa_cir_vec.push_back(0.0);
	}

	std::string out_file_name = rtk_data_path + "u_turn_record_daxia_sm.csv";
	io::CSVWriter rtk_csv(out_file_name);
	std::vector<std::string> head_string = {"x", "y", "theta", "kappa", "v", "s", "a"};
	rtk_csv.write_row<std::string>(head_string);
	for (int i = 0; i < x_cir_vec.size(); i++)
	{
		static double temp_s = 0;
		temp_s = temp_s + delta_s;
		std::vector<double> temp_vector = {x_cir_vec[i], y_cir_vec[i], theta_cir_vec[i], kappa_cir_vec[i], 10.0, temp_s, 0};
		rtk_csv.write_row<double>(temp_vector);
	}
	plt::plot(x_vec, y_vec);
	plt::plot(x_org_vec, y_org_vec);
	plt::plot(x_cir_vec, y_cir_vec);
	plt::show();
	plt::plot(theta_cir_vec);
	plt::show();
	plt::plot(kappa_cir_vec);
	plt::show();
	return 0;
}